////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2014 Kentaro Sekimoto  All rights reserved.
////////////////////////////////////////////////////////////////////////////

#include "FM3_GPIO.h"

#define MFS_MODE_MAX    3*8

static UINT8 mfs_sin[] = {
    0x21, 0x14, 0xb4,
    0x56, 0x11, 0xf0,
    0x72, 0x24, 0x17,
    0x75, 0x50, 0x48,
    0xd2, 0x1A, 0x05,
    0x60, 0x92, 0x36,
    0x53, 0x33, 0xf3,
    0x59, 0x4e, 0xb0
};

static UINT8 mfs_sot[] = {
    0x22, 0x15, 0xb5,
    0x57, 0x12, 0xf1,
    0x73, 0x25, 0x18,
    0x76, 0x51, 0x49,
    0xd1, 0x1b, 0x06,
    0x61, 0x93, 0x37,
    0x54, 0x32, 0xf4,
    0x5a, 0x4d, 0xb1
};

static UINT8 mfs_sck[] = {
    0x23, 0x16, 0xb6,
    0x58, 0x13, 0xf2,
    0x74, 0x26, 0x19,
    0x77, 0x52, 0x4a,
    0xd0, 0x1c, 0x07,
    0x62, 0x94, 0x38,
    0x55, 0x31, 0xf5,
    0x5b, 0x4c, 0xb2
};

static UINT8 mfs_epfr_shift [] = {
    4, 10, 16, 22, 36, 42, 48, 54
};

static UINT32 mfs_mode [] = {
    0x15, 0x2a, 0x3f
};

void FM3_MFS_SetMode(UINT32 mode)
{
    UINT32 port;
    UINT32 mask;
    UINT32 ch_num;
    UINT32 ch_mod;
    UINT32 m;

    if (mode > MFS_MODE_MAX)
        goto exit;
    if (mfs_sot[mode] == 0xff)
        goto exit;
    port = GPIO_PORT(mfs_sot[mode]);
    mask = GPIO_MASK(mfs_sin[mode]) + GPIO_MASK(mfs_sot[mode]) + GPIO_MASK(mfs_sck[mode]);
    GPIO_PFR(port) |= mask;
    ch_mod = mode % 3;
    ch_num = mode / 3;
    if ((ch_num == 5) && (ch_mod == 2)) {
        GPIO_EPFR(10) &= 0xfffffffb;
#if defined(FM3_MFS_WARNING)
        lcd_printf("warning MCLKOUT off\n");
#endif
    }
    if (ch_num < 4) {
        m = mfs_mode[ch_mod] << mfs_epfr_shift[ch_num];
        GPIO_EPFR(7) |= m;
#if defined(FM3_MFS_DEBUG)
        lcd_printf("CH%d_%d: EPFR[7] |= 0x%08x PFR[%x] |= 0x%04x\n", ch_num, ch_mod, m, port, mask);
#endif
    } else {
        m = mfs_mode[ch_mod] << (mfs_epfr_shift[ch_num] - 32);
        GPIO_EPFR(8) |= m;
#if defined(FM3_MFS_DEBUG)
        lcd_printf("CH%d_%d: EPFR[8] |= 0x%08x PFR[%x] |= 0x%04x\n", ch_num, ch_mod, m, port, mask);
#endif
    }
exit:
    return;
}
